#include "motor_a_position_config.h"
// 速度环框架
#include "position.h"
// 电机A电流环
#include "motor_a_foc_config.h"
// 电机A速度环
#include "motor_a_speed_config.h"


#define MA_POSITION_KP			0.0f	// 0.08f
#define MA_POSITION_KI			0.0f	// 0.000f
#define MA_POSITION_KD			0.0f	// 0.08f


float MOTORA_GetPosition(void);
void MOTORA_SetSpeed(float position);


POSITION_Control_Struct MotorA_POSITION_Control = 
{
    .isEnable 		= 0,	// 使能
    .sysTimer 		= 0,	// 系统时钟
    .pPID.kp 		= 0,	// PID
    .pPID.kd 		= 0,
    .pPID.ki 		= 0,
    .pPID.tar 		= 0,
    .pPID.pre 		= 0,
    .pPID.bias 		= 0,
    .pPID.lastBias	= 0,
    .pPID.err 		= 0,
    .pPID.out 		= 0,
    .pPID.outMax 	= 100,
	
    .pPID.cycle 	= 0.005,
	
    .GetPosition 	= MOTORA_GetPosition,	// 获取电机实际转速
    .SetMotorSpeed  = MOTORA_SetSpeed,		// 设置电机输出电流
};

/**
 * @brief   获取电机A位置
 *
 * @param   none
 *
 * @return  none
 */
float MOTORA_GetPosition(void)
{ 
    return MOTORA_GetAngle() -  180;
}

/**
 * @brief   设定电机A的目标位置
 *
 * @param   none
 *
 * @return  none
 */
void MOTORA_SetSpeed(float position)
{
    MOTORA_SPEED_Config_SetTar(position);
}

/* ================================================================ */

/**
 * @brief   设定电机A的目标位置
 *
 * @param   none
 *
 * @return  gSpeedPID_TimeCNT
 */
void MOTORA_POSITION_Config_Init(void)
{
	// 1. 使能位置环参数
    SetPositionPIDEnable(&MotorA_POSITION_Control, 1);
	
	// 2. 设置位置环参数
    SetPositionPIDParams(&MotorA_POSITION_Control, MA_POSITION_KP, MA_POSITION_KI, MA_POSITION_KD);
	
	// 3. 设置位置环目标值
	MOTORA_POSITION_Config_SetTar(0.0f);
}

/**
 * @brief   MOTORA 设定位置环目标值
 *
 * @param   none
 *
 * @return  none
 */
void MOTORA_POSITION_Config_SetTar(float tarPosition)
{
    SetPositionPIDTar(&MotorA_POSITION_Control, tarPosition);
}

/**
 * @brief   MOTORA 位置环循环
 *
 * @param   none
 *
 * @return  none
 */
void MOTORA_POSITION_Config_Loop(void)
{
    PositionPIDCycle(&MotorA_POSITION_Control);
}

/**
 * @brief   MOTORA 位置环信息输出
 *
 * @param   none
 *
 * @return  none
 */
void MOTORA_POSITION_Config_Printf(void)
{
	PositionPIDPrintf(&MotorA_POSITION_Control);
}

